package CamposDePotencial;

import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.io.InputStream;
import java.util.Properties;

import javax.vecmath.Point3d;

import estrategia.ParametrosPotenciales;
import estrategia.Predictor;
import estrategia.Utiles;
import estrategia.Vector;

public class CampoTangencialRepulsor extends CampoPotencial{

	private double radioCampo;
	private double minAlcance;
	private double Krepulsion;
	private double tamanioRobot;
	
	private double direccionObjetivo, direccionPerpendicularObjetivo, mObjetivo, mPerpendicular;
	private Objetivo objetivo;
	
	public CampoTangencialRepulsor(int idObjeto, Objetivo objetivo, Utiles u, Predictor predictor) {
		// TODO Auto-generated constructor stub
		super(idObjeto, u, predictor);
		this.objetivo = objetivo;
	}
	
	
	public void  cargarConstantes() {
		radioCampo = ParametrosPotenciales.radioCampoTangencial;
		minAlcance = ParametrosPotenciales.minAlcanceTangencial;
		Krepulsion = ParametrosPotenciales.KCampoTangencial;
		tamanioRobot = ParametrosPotenciales.tamanioRobot;	
	}
	
	public double hallarPendienteRecta(Point3d mipos, Point3d P) {			
		return (P.y - mipos.y)/(P.x - mipos.x);
	}
	
	
	public double evaluarRecta(double m, Point3d posRobot) {			
		return (m*posRobot.x - posRobot.y);
	}
	
	
	public double evaluarCircunferencia(Point3d posObjeto, Point3d posRobot) {
		double valor = Math.pow((posRobot.x - posObjeto.x), 2) + Math.pow((posRobot.y - posObjeto.y), 2); 
		return Math.sqrt(valor); 
	}
	
	
	public double calcularDireccion(Point3d mipos) throws IOException {
		double direccion = 0;
		
		Point3d posObj = predictor.predecirPosicion(idObjeto);
		direccionObjetivo = utiles.getAnguloTheta(posObj, objetivo.getPosicionObjetivo());		
		Point3d PObjetivo = objetivo.getPosicionObjetivo();
		mObjetivo = hallarPendienteRecta(mipos, PObjetivo);
		
		double anguloRobot = utiles.getAnguloTheta(mipos, posObj);
		
		//System.out.println("La direccion de mi robot mirado desde el objetivo es: " + utiles.getAnguloTheta(posObj, mipos));
		
		if (direccionObjetivo <= (Math.PI/2.0) && direccionObjetivo >= -(Math.PI/2.0)) {
			if (Math.signum((mObjetivo*posObj.x -posObj.y ) - evaluarRecta(mObjetivo, mipos)  ) >= 0) {
				// Tengo que sumar 90
				direccion = anguloRobot + Math.PI/2;
				if (direccion > Math.PI)
					direccion = -Math.PI + (direccion - Math.PI);
			} else {
				// Tengo que restar 90
				direccion = anguloRobot - Math.PI/2;
				if (direccion < -Math.PI)
					direccion = Math.PI + (direccion + Math.PI);
			}
		} else {
			if (Math.signum( (mObjetivo*posObj.x -posObj.y ) - evaluarRecta(mObjetivo, mipos)) >= 0) {
				// Tengo que restar 90
				direccion = anguloRobot - Math.PI/2;
				if (direccion < -Math.PI)
					direccion = Math.PI + (direccion + Math.PI);
			} else {
				// Tengo que sumar 90
				direccion = anguloRobot + Math.PI/2;
				if (direccion > Math.PI)
					direccion = -Math.PI + (direccion - Math.PI);
				
			}
		}
			
		
		return direccion;
	}
	public double getMagnitudFuerza(Point3d mipos, Point3d posObj) {
		double magnitud = 0;
		double distancia = utiles.distanciaEuclidea(mipos, posObj);
		distancia = distancia - tamanioRobot;		
		
		if (evaluarCircunferencia(posObj, mipos) <= radioCampo) { 
				magnitud = Krepulsion*((1/distancia) - (1/radioCampo));
		}
		
		
		return magnitud;
	}
	
	
	public Vector obtenerFuerza(Point3d mipos) throws IOException{
		cargarConstantes();
		
		double direccion = calcularDireccion(mipos);
		
		double magnitud = getMagnitudFuerza(mipos, predictor.predecirPosicion(idObjeto));
		
		//System.out.println("REPULSOR tangencial " + idObjeto + ": direccion = " + direccion + " magnitud = " + magnitud);
		 
		Vector resultante = new Vector(magnitud, direccion);  
		
		resultante.setMagnitud(Math.abs(magnitud));
		
		return resultante;
	};
}

